#ifndef __ROBOT_KUKA_CARMCLIENT_H__
#define __ROBOT_KUKA_CARMCLIENT_H__

#include <robot/robot.h>
#include <robot/kuka/enums.h>
#include <robot/kuka/ckinematics.h>
#include <blackbox/bbox.h>
#include <blackbox/proxies.h>
#include <blackbox/cdatain.h>
#include <blackbox/cdataout.h>

namespace buola { namespace robot { namespace kuka {

using namespace buola;
    
class CArmClient : public blackbox::BBox
{
public:
    CArmClient();
    ~CArmClient();
    
    void Init(const blackbox::CBoxProxy &pBox=blackbox::CClientProxy("bbarm@saruman")["arm"]);

    void GetPose(std::vector<double> &pValues);
    void GetPose(CArmPose &pPose);
    void GetPose(mat::CVec3d &pPos,mat::C3DRotation &pRot);
    mat::CVec3d GetPos();
    mat::C3DRotation GetRot();
    
    void SetGoal(const mat::CVec3d &pPos,bool pCartesian=false);
    void SetGoal(const mat::C3DRotation &pRot,bool pCartesian=false);
    void SetGoal(const mat::CVec3d &pPos,const mat::C3DRotation &pRot,bool pCartesian=false);
    void SetGoal(const CArmPose &pPos,bool pCartesian=false);
    void SetGoal(const std::vector<double> &pJoints,bool pCartesian=false);
    void SetGoal(int pJoint,double pValue,bool pCartesian=false);

    void DoCorrection(const mat::CVec3d &pPos,const mat::C3DRotation &pRot=mat::C3DRotation(nZero));

    void Stop();

    void StartSinTest(int pJoint,double pAmplitude);

    void SetMaxSpeed(double pLinear,double pRot);

    bool IsMoving();
    void WaitForStop();

private:
    std::unique_ptr<blackbox::CBoxProxy> mArm;
    blackbox::CArrayOut<double> oJoints;
    blackbox::CArrayIn<double> iJoints;
    blackbox::CDataOut<mat::CVec3d> oPosition;
    blackbox::CDataOut<mat::CQuat_d> oOrientation;
    blackbox::CEnablerOut<EArmMode> oArmMode;
    blackbox::CDataIn<int> iStatus;
};

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }

#endif
